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Resumen 



Este documcnto es una tesis en el tema de planificacion de caminos uniagente y en 
Imea, para ambientes continues, impredecibles y altamente dinamicos. El problema es 
encontrar y recorrer un camino sin colisiones para un robot holonomico, sin restric- 
ciones kinodinamicas, moviendose en un ambiente con varios obstaculos o adversaries 
moviendose impredeciblemente. Se asume la disponibilidad de informacion perfecta del 
entorno en todo momento. 

Varias variantes estaticas y dinamicas del algoritmo "Rapidly Exploring Random 
Trees" (RRT) sc cxploran, asi como tambien un algoritmo evolutivo para planificacion 
en ambientes dinamicos llamado "Evolutionary Planner/Navigator." Se propone una 
combinacion de ambos algoritmos para superar las falencias de ambos y luego una 
combinacion de RRT para planificacion inicial y biisqueda local informada para nave- 
gacion, sumado a una heuristica voraz simple para optimizacion. Se demuestra que esta 
combinacion de tecnicas simples produce mejores respuestas en ambientes altamente 
dinamicos que las variantes RRT estandar. 

Palabras Claves: Inteligencia artificial, planificacion de rutas, RRT, multi-etapa, 
biisqueda local informada, heuristica voraz, algoritmos evolutivos 
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Abstract 



This document is a thesis on the subject of single-agent on-hne path planning in con- 
tinuous, unpredictable and highly dynamic environments. The problem is finding and 
traversing a collision-free path for a holonomic robot, without kinodynamic restrictions, 
moving in an environment with several unpredictably moving obstacles or adversaries. 
The availability of perfect information of the environment at all times is assumed. 

Several static and dynamic variants of the Rapidly Exploring Random Trees (RRT) 
algorithm are explored, as well as an evolutionary algorithm for planning in dynamic 
environments called the Evolutionary Planner/Navigator. A combination of both kinds 
of algorithms is proposed to overcome shortcomings in both, and then a combination 
of a RRT variant for initial planning and informed local search for navigation, plus 
a simple greedy heuristic for optimization. We show that this combination of simple 
techniques provides better responses to highly dynamic environments than the RRT 
extensions. 

Keywords: Artificial intelligence, motion planning, RRT, Multi-stage, informed 
local search, greedy heuristics, evolutionary algorithms 
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Chapter 1 
Introduction 



The dynamic path-planning problem consists in finding a suitable plan for each new 
configuration of the environment by recomputing a collision-free path using the new 
information available at each time step |HA92j . This kind of problem has to be solved 
for example by a robot trying to navigate through an area crowded with people, such 
as a shopping mall or supermarket. The problem has been widely addressed in its 
several flavors, such as cellular decomposition of the configuration space |Ste95] . partial 
environmental knowledge |Ste94j . high-dimensional configuration spaces |KSL096] or 
planning with non-holonomic constraints |LKJ99 ]. However, even simpler variations 
of this problem are complex enough that they can not be solved with deterministic 
techniques, and therefore are worthy of study. 

This thesis is focused on algorithms for finding and traversing a collision-free path 
in two dimensional space, for a holonomic robot ^, without kinodynamic restrictions^, 
in a highly dynamic environment, but for comparison purposes three different scenarios 
will be tested: 

• Several unpredictably moving obstacles or adversaries. 

• Partially known environment, where some obstacles become visible when the 
robot approaches each one of them. 



holonomic robot is a robot in which the controllable degrees of freedom is equal to the total 
degrees of freedom. 

^Kinodynamic planning is a problem in which velocity and acceleration bounds must be satisfied 
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• Totally unknown environment, where every obstacle is initially invisible to the 
planner, and only becomes visible when the robot approaches it. 

Besides the obstacles in the second and third scenario we assume that we have perfect 
information of the environment at all times. 

We will focus on continuous space algorithms and will not consider algorithms that 
use a discretized representation of the configuration space^, such as D* |Ste95j . because 
for high dimensional problems the configuration space becomes intractable in terms of 
both memory and computation time, and there is the extra difficulty of calculating 
the discretization size, trading off accuracy versus computational cost. Only single 
agent algorithms will be considered here. On-line as well as off-line algorithms will 
be studied. An on-line algorithm is one that is permanently adjusting its solution as 
the environment changes, while an off-line algorithm computes a solution only once 
(however, it can be executed many times). 

The offiine Rapidly-Exploring Random Tree (RRT) is efficient at finding solutions, 
but the results are far from optimal, and must be post-processed for shortening, smooth- 
ing or other qualities that might be desirable in each particular problem. Furthermore, 
replanning RRTs are costly in terms of computation time, as are evolutionary and 
cell-decomposition approaches. Therefore, the novelty of this work is the mixture of 
the feasibility benefits of the RRTs, the repairing capabilities of local search, and the 
computational inexpensiveness of greedy algorithms, into our lightweight multi-stage 
algorithm. Our working hypothesis will be that a multi-stage algorithm, using differ- 
ent techniques for initial planning and navigation, outperforms current probabilistic 
sampling techniques in highly dynamic environments 

1.1 Problem Formulation 

At each time-step, the problem could be defined as an optimization problem with 
satisfiability constraints. Therefore, given a path our objective is to minimize an 
evaluation function (i.e., distance, time, or path-points), with the Cfree constraint. 

■^thc space of possible positions that a physical system may attain 
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Formally, let the path p = piP2 ■ ■ - Pn be a sequence of points, where pi G is a n- 
dimensional point {pi = qinit,Pn = ^goai). Of & O the set of obstacles positions at time 
t, and eval: x (9 M an evaluation function of the path depending on the object 
positions. Our ideal objective is to obtain the optimum p* path that minimizes our 
eval function within a feasibility restriction in the form 

p* = argmin[eval(p. Of)] with feas(p, O^) = Cfree (1-1) 
p 

where feas(-, ■) is a feasibility function that equals Cfree if the path p is collision free 
for the obstacles Ot. For simplicity, we use very naive eval(-, ■) and feas(-, ■) functions, 
but our approach could be extended easily to more complex evaluation and feasibility 
functions. The feas(p, Ot) function used assumes that the robot is a point object in 
space, and therefore if no segments PiPi+l of the path collide with any object oj G Ot, 
we say that the path is in C^ee- The eval(p, Ot) function is the length of the path, i.e., 
the sum of the distances between consecutive points. This could be easily changed to 
any other metric such as the time it would take to traverse this path, accounting for 
smoothness, clearness or several other optimization criteria. 



1.2 Document Structure 



In the following sections we present several path planning methods that can be applied 



to the problem described above. In section 2A_ we review the basic offline, single-query 
RRT, a probabilistic method that builds a tree along the free configuration space until 
it reaches the goal state. Afterwards, we introduce the most popular replanning vari- 



ants of RRT: Execution Extended RRT (ERRT) in section 2.3 Dynamic RRT (DRRT) 
in section 2.4 and Multipartite RRT (MP-RRT) in section 2.5 The Evolutionary Plan- 



ner/Navigator (EP/N), along with some variants, is presented in section 2.8 Then, in 



section 3.1 we present a mixed approach, using a RRT to find an initial solution and 



the EP/N to navigate, and finally, in section 3.2 we present our new hybrid multi-stage 



algorithm, that uses RRT for initial planning and informed local search for navigation, 
plus a simple greedy heuristic for optimization. Experimental results and compar- 
isons that show that this combination of simple techniques provides better responses 
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to highly dynamic environments than the standard RRT extensions are presented in 
section 14.31 The conclusions and further work are discussed in section |5l 
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Chapter 2 
State of the Art 



In this chapter we present several path planning methods that can be applied to the 
problem described above. First we will introduce variations of the Rapidly-Exploring 
Random Tree (RRT), a probabilistic method that builds a tree along the free config- 
uration space until it reaches the goal state. This family of planners is fast at finding 
solutions, but the solutions are far from optimal, and must be post-processed for short- 
ening, smoothing or other qualities that might be desirable in each particular problem. 
Furthermore, replanning RRTs are costly in terms of computation time. We then in- 
troduce an evolutionary planner with somewhat opposite qualities: It is slow in finding 
feasible solutions in difficult maps, but efficient at replanning when a feasible solution 
has already been found. It can also optimize the solution according to any given fitness 
function without the need for a post-processing step. 



2.1 Rapidly- Exploring Random Tree 

One of the most successful probabilistic methods for offline path planning currently 
in use is the Rapidly- Exploring Random Tree (RRT), a single-query planner for static 
environments, first introduced in |Lav98j . RRTs works towards finding a continuous 
path from a state ginit to a state ggoai in the free configuration space Cfree by building 
a tree rooted at ginit- A new state grand is uniformly sampled at random from the 
configuration space C . Then the nearest node, gnear, in the tree is located, and if grand 
and the shortest path from grand to gnear are in Cfree, then grand is added to the tree 
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(algorithm [T|. The tree growth is stopped when a node is found near ggoai- To speed 
up convergence, the search is usually biased to ggoai with a small probability. 

In |KL00j . two new features are added to RRT. First, the EXTEND function (algo- 
rithm [2]) is introduced, which instead of trying to add grand directly to the tree, makes 
a motion towards grand and tests for collisions. 

Algorithm 1 BuildRRT(ginit, ggoai) 
1: T ^ empty tree 

2: T.init(ginit) 

3: while Distance(T, ggoai) > threshold do 
4: grand RandomConfig() 
5: Extend(r, grand) 

6: return T 



Algorithm 2 Extend(T, g) 



1: gncar ^ NearestNeighbor(g, T) 

2: if NewConfig(g, gnear, gnew) then 

3: T. add_vertex(gnew) 

4: T. add_edge(gnear, gnew) 

5: if gnew = g then 

6: return Reached 

7: else 

8: return Advanced 

9: return Trapped 



Then a greedier approach is introduced (the CONNECT function, shown in algo- 
rithms [s] and |4]) , which repeats EXTEND until an obstacle is reached. This ensures 
that most of the time we will be adding states to the tree, instead of just rejecting new 
random states. The second extension is the use of two trees, rooted at ginit and ggoai, 



which are grown towards each other (see figure 2.1). This significantly decreases the 
time needed to find a path. 
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Figure 2.1: RRT during execution 



Algorithm 3 RRTConnectPlanner(ginit, 5'goai) 
1: Ta ^ tree rooted at gmit 

2: Tfc ^ tree rooted at ggoai 

3: Ta.init(ginit) 

4: Tfe.init(ggoai) 

5: for A; = 1 to i^' do 

6: Q'rand ^ RandomConfig() 

7: if not (Extend(T(j, grand) = Trapped) then 

8: if Connect (Tfe, gnew) = Reached then 

9: return Path(T„,T6) 

10: Swap(Ta,Tb) 
11: return Failure 

Algorithm 4 Connect (T,g) 
1: repeat 

2: S ^ Extend(T, q) 
3: until (5* 7^ Advanced) 
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2.2 Retraction-Based RRT Planner 



The Retraction-based RRT Planner presented in |ZM08] aims at improving the per- 
formance of the standard offline RRT in static environments with narrow passages. 
The basic idea of the Optimize(gr, Qn) function in algorithm [s] is to iteratively retract a 
randomly generated configuration that is in Cobs to the closest boundary point in Cfree- 
So, instead of using the standard extension that tries to extend in a straight line from 
Qnear to ^rand, it exteuds from ^near to the closcst poiut in Cf^ee to ^rand- This givcs more 
samples in narrow passages. This technique could easily be applied to on-line RRT 
planners. 



Algorithm 5 Retraction-based RRT Extension 



1: 


Qr ^ a. random configuration in Cspace 


2: 


Qn ^ the nearest neighbor of qr in T 


3: 


if CollisionFree(g„, g,.) then 


4: 


T. addVertex(gr) 


5: 


T. addEdge(g„, g^) 


6: 


else 


7: 


5* ^ Optimize(gr, Qn) 


8: 


for all Qi E S do 


9: 


Standard RRT Extension (T, gj) 


10: 


return T 



2.3 Execution Extended RRT 

The Execution Extended RRT presented in |BV02j introduces two extensions to RRT 
to build an on-line planner, the waypoint cache and adaptive cost penalty search, 
which improve re-planning efflciency and the quality of generated paths. ERRT uses a 



kd-tree (see section 2.7) to speed nearest neighbor look-up, and does not use bidirec- 
tional search. The waypoint cache is implemented by keeping a constant size array of 
states, and whenever a plan is found, all the states in the plan are placed in the cache 
with random replacement. Then, when the tree is no longer valid, a new tree must be 
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grown, and there are three possibihties for choosing a new target state, as shown in 
algorithm [6| which is used instead of RandomConfig() in previous algorithms. With 
probability P[goal], the goal is chosen as the target; with probability P[waypoint], a 
random waypoint is chosen, and with the remaining probability a uniform state is 
chosen as before. In |BV02j the values used are P[goal]= 0.1 and P[waypoint]= 0.6. 

Another extension is adaptive cost penalty search, where the planner adaptively 
modified a parameter to help it find shorter paths. A value of 1 for beta will always 
extend from the root node, while a value of is equivalent to the original algorithm. 
However, the paper |BV02] lacks implementation details and experimental results on 
this extension. 

Algorithm 6 ChooseTarget(g, goal) 
1: p ^ UniformRandom(0.0, 1.0) 

2: i UniformRandom(0.0, NumWayPoints) 

3: if < p < GoalProb then 

4: return ggoai 

5: else if GoalProb < p < GoalProb + WayPointProb then 

6: return WayPoint Cache [i] 

7: else if GoalProb + WayPointProb < p < 1 then 

8: return RandomConfig() 



2.4 Dynamic RRT 

The Dynamic Rapidly-Exploring Random Tree described in |FKS06j is a probabilistic 
analog to the widely used D* family of algorithms. It works by growing a tree from 
Qgoai to ginit, as shown in algorithm [7[ This has the advantage that the root of the tree 
does not have to be moved during the lifetime of the planning and execution. In some 
problem classes the robot has limited range sensors, thus moving or newly appearing 
obstacles will be near the robot, not near the goal. In general this strategy attempts 
to trim smaller branches that are farther away from the root. When new information 
concerning the configuration space is received, the algorithm removes the newly-invalid 



branches of the tree (algorithms ^ and 10), and grows the remaining tree, focusing. 



with a certain probability (empirically tuned to 0.4 in |FKS06j ) to a vicinity of the 
recently trimmed branches, by using the waypoint cache of the ERRT (algorithm [6]). 
In experiments presented in |FKS06] DRRT vastly outperforms ERRT. 

Algorithm 7 DRRT() 
1: Q'robot ^ the current robot position 

2: T ^ BuildRRT(ggoai, grobot) 

3: while grobot 7^ ggoai do 

4- ^next 

^ Parent (grobot) 

5: Move from g^bot to gnext 

6: for all obstacles that changed O do 
7: InvalidateNodes(O) 

8: if Solution path contains an invalid node then 
9: ReGrowRRTO 



Algorithm 8 ReGrowRRT() 
1: TrimRRTO 

2: GrowRRTO 



Algorithm 9 TrimRRT() 

1: S 4- 0,2 ^ 1 

2: while i < T. size() do 

3: qi <— T. node(z) 

4: gp <— Parent (gj) 

5: if gp.flag = INVALID then 

6: gi.flag ^ INVALID 

7: if g,.flag ^ INVALID then 

8: S^S[j{q,} 

9: i + 1 

10: T ^ CreateTreeFromNodes(5') 
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Algorithm 10 InvalidateNodes(o6s)f:ac/e) 



1: E ^ FindAffectedEdges (obstacle) 
2: for all e G -E do 
3: Qe ^ ChildEndpointNode(e) 
4: ge-flag ^ INVALID 

2.5 Multipartite RRT 

Multipartite RRT presented in |ZKB07] is another RRT variant which supports plan- 
ning in unknown or dynamic environments. MP-RRT maintains a forest F of discon- 
nected sub-trees which lie in Cfree, but which are not connected to the root node groot 
of T, the main tree. At the start of a given planning iteration, any nodes of T and F 
which are no longer valid are deleted, and any disconnected sub-trees which are created 



as a result are placed into F (as seen in algorithms 11 and 12). With given probabil 



ities, the algorithm tries to connect T to a new random state, to the goal state, or to 



the root of a tree in F (algorithm 13 ). In |ZKB07] . a simple greedy smoothing heuristic 
is used, that tries to shorten paths by skipping intermediate nodes. The MP-RRT is 
compared to an iterated RRT, ERRT and DRRT, in 2D, 3D and 4D problems, with 
and without smoothing. For most of the experiments, MP-RRT modestly outperforms 
the other algorithms, but in the 4D case with smoothing, the performance gap in favor 
of MP-RRT is much larger. The authors explained this fact due to MP-RRT being able 
to construct much more robust plans in the face of dynamic obstacle motion. Another 
algorithm that utilizes the concept of forests is Reconfigurable Random Forests (RRF) 
presented in |LS02] . but without the success of MP-RRT. 

2.6 Rapidly Exploring Evolutionary Tree 

The Rapidly Exploring Evolutionary Tree, introduced in |MWS07] uses a bidirectional 



RRT and a kd-tree (see section 2.7) for efficient nearest neighbor search. The modifica- 



tions to the Extend() function are shown in algorithm 14 The re-balancing of a kd-tree 
is costly, and in this paper a simple threshold on the number of nodes added before 
re-balancing was used. The authors suggest using the method described in |AL02j and 
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Algorithm 11 MPRRTSearch(ginit) 
1: T 4— the previous search tree, if any 

2: F ^ the previous forest of disconnected sub-trees 

3: feit ^ the initial state 

4: if T = then 

5: Q'root Q'mit 

6: Insert (groot,T) 
7: else 

8: PruneAndPrepend(T, F, qinn) 

9: if TreeHasGoal(T) then 
10: return true 
11: while search time/space remaining do 
12: gncw ^ SelectSample(F) 
13: gncar ^ NearcstNcighbor (gnew.r) 
14: if Qncw £ then 

15: ^connect ^ Connect(5'ncar) lyncw) 

16: if ^connect SLiid TreeHasGoal(T) then 

17: return true 

18: else 

19: ^extend ^ Extend(gnear, gnew) 

20: if ^extend ^nd IsGoal(gnew) then 
21: return true 

22: return false 

used in |BV02] to improve the search speed. The novelty in this algorithm comes from 
the introduction of an evolutionary algorithm |BFM97j that builds a population of 
biases for the RRTs. The genotype of the evolutionary algorithm consists of a single 
robot configuration for each tree. This configuration is sampled instead of the uniform 
distribution. To balance exploration and exploitation, the evolutionary algorithm was 
designed with 50% elitism. The fitness function is related to the number of left and 
right branches traversed during the insertion of a new node in the kd-tree. The goal 
is to introduce a bias to the RRT algorithm which shows preference to nodes created 
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Algorithm 12 PruneAndPrepend(T, F, ginit) 
1: for all g G T,F do 

2: if not NodeValid(g) then 

3: KillNode(g) 

4: else if not Action Valid(g) then 

5: SplitEdge(g) 

6: if not T = and groot 7^ Qmit then 

7: if not ReRoot(r, ginit) then 

8: F ^ F[jT 

9: T. init(ginit) 

Algorithm 13 SelectSample(-F) 
1: p ^ Random(0, 1) 

2: if p < pgoal then 

3- Qncw ^ ?goal 

4: else if p < (pgoai + Pforcst) and not Empty(F) then 
5: gncw *— q ^ SubTreeRoots(F) 
6: else 

7: gncw ^ RandomState() 
8: return q^^w 



away from the center of the tree. The authors suggest combining RET with DRRT or 
MP-RRT. 

2.7 Multidimensional Binary Search Trees 

The kd-tree, first introduced in |Ben75] . is a binary tree in which every node is a 
k-dimensional point. Every non-leaf node generates a splitting hyperplane that divides 
the space into two subspaces. In the RRT algorithm, the number of points grows incre- 
mentally, unbalancing the tree, thus slowing nearest-neighbor queries. Re-balancing a 
kd-tree is costly, so in |AL02j the authors present another approach: A vector of trees 
is constructed, where for n points there is a tree that contains 2* points for each " 1" in 
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Algorithm 14 ExtendToTarget(T) 
1: static p: population, inc <— 1 

2: p': temporary population 

3: if inc > length(p) then 

4: SortByFitness(p) 

5: p' ^ null 

6: for alH e p do 

7: if i is in upper 50% then 

8: Addlndividual(i,p') 

9: else 

10: i RandomState() 

11: Addlndividual(i,p') 

12: p <^ p' 

13: inc ^ 1 

14: qr j9(inc) 

15: gnear ^ Nearest (T, g^) 

16: gnew ^ Extend (T, gnear) 

17: if gnew 7^ then 

18: AddNode(r, ^new) 

19: AssignFitness(p(inc), fitness(gnew) 

20: else 

2 1 : AssignFitness (p (inc) , 0) 
22: return ^new 

the i^^ place of the binary representation of n. As bits are cleared in the representation 
due to increasing n, the trees are deleted, and the points are included in a tree that 
corresponds to the higher-order bit which is changed to " 1" . This general scheme incurs 
in logarithmic-time overhead, regardless of dimension. Experiments show a substantial 
performance increase compared to a naive brute-force approach. 
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2.8 Evolutionary Planner/Navigator 

An evolutionary algorithm |BFM97] is a generic population-based meta-heuristic opti- 
mization algorithm. It is inspired in biological evolution, using methods such as indi- 
vidual selection, reproduction and mutation. The population is composed of candidate 
solutions and they are evaluated according to a fitness function. 

The Evolutionary Planner/Navigator presented in |XMZ96] . |XMZT97j . and |TX97] 
is an evolutionary algorithm for path finding in dynamic environments. A high level 



description is shown in algorithm 15 A difference with RRT is that it can optimize 
the path according to any fitness function defined (length, smoothness, etc), without 
the need for a post-processing step. Experimental tests have shown it has good per- 
formance for sparse maps, but no so much for difficult maps with narrow passages or 
too crowded with obstacles. However, when a feasible path is found, it is very efficient 
at optimizing it and adapting to the dynamic obstacles. Every individual in the pop- 
ulation is a sequence of nodes, representing nodes in a path consisting of straight-line 
segments. Each node consists of an {x,y) pair and a state variable b with information 
about the feasibility of the point and the path segment connecting it to the next point. 
Individuals have variable length. 

Since a path p can be either feasible or unfeasible, two evaluation functions are 



used. For feasible paths (equation 2.1), the goal is to minimize distance traveled. 



maintain a smooth trajectory and satisfy a clearance requirement (the robot should 



not approach the obstacles too closely). For unfeasible paths, we use equation 2.2 , taken 
from |Xia97j . where /z is the number of intersections of a whole path with obstacles 
and 7] is the average number of intersections per unfeasible segment. 

eval/(p) = Wd ■ dist(p) + Wg ■ smooth(j9) + Wc ■ clear(p) (2.1) 
eval„(p) = fi + r] (2.2) 



EP/N uses eight different operators, as shown in figure 2.2| (description taken 
from |XMZ96] ): 

Crossover: Recombines two (parent) paths into two new paths. The parent paths 
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Algorithm 15 EP/N 



1: 


P{t): population at generation t 


2: 


t ^ 


3: 


lnitialize(P(t)) 


4: 


Evaluate(P(t)) 


5: 


while (not termination-condition) do 


6: 




7: 


Select operator Oj with probability pj 


8: 


Select parent (s) from P{t) 


9: 


Produce offspring applying oj to selected parent (s) 


10: 


Evaluate offspring 


11: 


Replace worst individual in P{t) by new offspring 


12: 


Select best individual p from P(t) 


13: 


if Feasible (p) then 


14: 


Move along path p 


15: 


Update all individuals in P(t) with current position 


16: 


if changes in environment then 


17: 


Update map 


18: 


Evaluate(P(i)) 


19: 





are divided randomly into two parts respectively and recombined: The first part 
of the first path with the second part of the second path, and the first part of 
the second path with the second part of the first path. Note that there can be 
different numbers of nodes in the two parent paths. 

MutateA: Used to fine tune node coordinates in a feasible path for shape adjustment. 
This operator randomly adjusts node coordinates within some local clearance of 
the path so that the path remains feasible afterwards. 

Mutate.2: Used for large random changes of node coordinates in a path, which can 
be either feasible or unfeasible. 
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Crossover Mutation_l Mutation_2 Insert_Delete 




Figure 2.2: The roles of the genetic operators 

Insert- Delete: Operates on an unfeasible path by inserting randomly generated new 
nodes into unfeasible path segments and deleting unfeasible nodes (i.e., path 
nodes that are inside obstacles). 

Delete: Deletes nodes from a path, which can be either feasible or unfeasible. If the 
path is unfeasible, the deletion is done randomly. Otherwise, the operator decides 
whether a node should definitely be deleted based on some heuristic knowledge, 

and if a node is not definitely deletable, its deletion will be random. 

Swap: Swaps the coordinates of randomly selected adjacent nodes in a path, which 
can be either feasible or unfeasible. 

Smooth: Smoothens turns of a feasible path by "cutting corners," i.e., for a selected 
node, the operator inserts two new nodes on the two path segments connected to 
that node respectively and deletes that selected node. The nodes with sharper 
turns are more hkely to be selected. 

Repair: Repairs a randomly selected unfeasible segment in a path by "pulling" the 
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segment around its intersecting obstacle. 



The probabilities of using each operator is set randomly at the beginning, and then are 
updated according to the success ratio of each operator, so more successful operators 
are used more often, and automatically chosen according to the instance of the problem, 
eliminating the difficult problem of hand tuning the probabilities. 

In |TX97] . the authors include a memory buffer for each individual to store good 
paths from its ancestors, which gave a small performance gain. 

In |EAA04] . the authors propose strategies for improving the stability and con- 
trolling population diversity for a simplified version of the EP/N. An improvement 
proposed by the authors in |XMZT97] is using heuristics for the initial population. 



instead of random initialization. We will consider this improvement in section 3.1 



Other evolutionary algorithms have also been proposed for similar problems, in |NG04j 
a binary genetic algorithm is used for an offline planner, and |NVTK03] presents an 
algorithm to generate curved trajectories in 3D space for an unmanned aerial vehicle. 

EP/N has been adapted to an 8-connected grid model in |AR08j (with previous 
work in |AR05j and |Alf05j ) . The authors study two different crossover operators and 
four asexual operators. Experimental results for this new algorithm (EvP) in static 
unknown environments show that it is faster than EP/N. 
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Chapter 3 

Proposed Techniques 



3.1 Combining RRT and EP/N 

As mentioned in section [2| RRT variants produce suboptimal solutions, which must 
later be post-processed for shortening, smoothing or other desired characteristics. On 



the other hand, EP/N, presented in section 2.8, can optimize a solution according to 
any given fitness function. However, this algorithm is slower at finding a first feasible 
solution. In this section we propose a combined approach, that uses RRT to find an 
initial solution to be used as starting point for EP/N, taking advantage of the strong 
points of both algorithms. 

3.1.1 The Combined Strategy 
Initial Solution 



EP/N as presented in section 2.8 can not find feasible paths in a reasonable amount of 
time in any but very sparse maps. For this reason, RRT will be used to generate a first 
initial solution, ignoring the effects produced by dynamic objects. This solution will be 
in the initial population of the evolutionary algorithm, along with random solutions. 



Feasibility and Optimization 

EP/N is the responsible of regaining feasibility when it is lost due to a moving obstacle 
or a new obstacle found in a partially known or totally unknown environment. If a 
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feasible solution can not be found in a given amount of time, the algorithm is restarted, 
keeping its old population, but adding a new individual generated by RRT. 



3.1.2 Algorithm Implementation 

Algorithm 16 Main() 
1: 9robot ^ is the current robot position 

2: Qgoai ^ is the goal position 

3: while grobot 7^ ggoai do 

4: updateWorld(time) 

5: processRRTEPN(time) 



The combined RRT-EP/N algorithm proposed here works by alternating environ- 



ment updates and path planning, as can be seen in algorithm 16 The first stage of 



the path planning (see algorithm 17) is to find an initial path using a RRT technique, 
ignoring any cuts that might happen during environment updates. Thus, the RRT 
ensures that the path found does not collide with static obstacles, but might collide 
with dynamic obstacles in the future. When a first path is found, the navigation is 



done by using the standard EP/N as shown in algorithm 15 



3.2 A Simple Multi-stage Probabilistic Algorithm 

In highly dynamic environments, with many (or a few but fast) relatively small mov- 
ing obstacles, regrowing trees are pruned too fast, cutting away important parts of the 
trees before they can be replaced. This dramatically reduces the performance of the 
algorithms, making them unsuitable for these classes of problems. We believe that bet- 
ter performance could be obtained by slightly modifying a RRT solution using simple 
obstacle-avoidance operations on the new colliding points of the path by informed local 
search. The path could be greedily optimized if the path has reached the feasibility 
condition. 
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Algorithm 17 processRRTEPN(time) 

1: (Zrobot ^ the current robot position 

2: 5'start ^ the starting position 

3: 5'goai ^ the goal position 

4: Tinit ^ the tree rooted at the robot position 

5: T'goai ^ the tree rooted at the goal position 

6: path <— the path extracted from the merged RRTs 

7: Q'robot ^ Q'start 

8: Tinit- init(grobot) 

9: Tgoai.init(g 

goal ) 

10: while time elapsed < time do 
11: if First path not found then 

12: RRT(Tinit, Tgoal) 

13: else 

14: EP/N() 



3.2.1 A Multi-stage Probabilistic Strategy 



If solving equation is not a simple task in static environments, solving dynamic 
versions turns out to be even more difficult. In dynamic path planning we cannot 
wait until reaching the optimal solution because we must deliver a "good enough" plan 
within some time restriction. Thus, a heuristic approach must be developed to tackle 



the on-line nature of the problem. The heuristic algorithms presented in sections 2.3 



2.4 and 2.5 extend a method developed for static environments, which produces poor 



response to highly dynamic environments and unwanted complexity of the algorithms. 

We propose a multi-stage combination of simple heuristic and probabilistic tech- 
niques to solve each part of the problem: Feasibility, initial solution and optimization. 



Feasibility 

The key point in this problem is the hard constraint in equation IJ^ which must be 
met before even thinking about optimizing. The problem is that in highly dynamic 
environments a path turns rapidly from feasible to unfeasible — and the other way 
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around — even if our path does not change. We propose a simple informed local search 
to obtain paths in Cfree- The idea is to randomly search for a Cfrce path by modifying 
the nearest colliding segment of the path. As we include in the search some knowledge 
of the problem, the informed term is coined to distinguish it from blind local search. 
The details of the operators used for the modification of the path are described in 



section 3.2.2 If a feasible solution can not be found in a given amount of time, the 
algorithm is restarted, with a new starting point generated by a RRT variant. 



Initial Solution 

The problem with local search algorithms is that they repair a solution that it is 
assumed to be near the feasibility condition. Trying to produce feasible paths from 
scratch with local search (or even with evolutionary algorithms |XMZT97] ) is not a 
good idea due the randomness of the initial solution. Therefore, we propose feeding 
the informed local search with a standard RRT solution at the start of the planning. 



as can be seen in figure 3.1 



Optimization 

Without an optimization criterion, the path could grow infinitely large in time or size. 
Therefore, the eval(-, ■) function must be minimized when a (temporary) feasible path 
is obtained. A simple greedy technique is used here: We test each point in the solution 
to check if it can be removed maintaining feasibility; if so, we remove it and check the 
following point, continuing until reaching the last one. 



3.2.2 Algorithm Implementation 

Algorithm 18 Main() 
1: Q'robot the current robot position 

2: 5'goai ^ the goal position 

3: while grobot 7^ ggoai do 

4: updateWorld(time) 

5: processMultiStage(time) 
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The multi-stage algorithm proposed in this thesis works by alternating environment 



updates and path planning, as can be seen in algorithm 18 The first stage of the path 



planning (see algorithm 19) is to find an initial path using a RRT technique, ignoring 
any cuts that might happen during environment updates. Thus, RRT ensures that 
the path found does not collide with static obstacles, but might collide with dynamic 
obstacles in the future. When a first path is found, the navigation is done by alternating 



a simple informed local search and a simple greedy heuristic as shown in figure 3.1 



Algorithm 19 processMultiStage(time) 



1: Q'robot ^ is the current robot position 

2: g'start ^ is the starting position 

3: 5'goai is the goal position 

4: Tinit ^ is the tree rooted at the robot position 

5: Tgoai ^ is the tree rooted at the goal position 

6: path is the path extracted from the merged RRTs 

7: Q'robot ^ Q'start 

8: Tinit. init(grobot) 

9: Tgoai.init(g 

goal I 

10: while time elapsed < time do 
11: if First path not found then 

12: RRT(Tinit, Tgoal) 

13: else 

14: if path is not collision free then 

15: firstCol <— collision point closest to robot 

16: arc(path, firstCol) 

17: mut(path, firstCol) 

18: postProcess(path) 

The second stage is the informed local search, which is a two step function composed 



by the arc and mutate operators (algorithms 20 and 21). The first one tries to build a 



square arc around an obstacle, by inserting two new points between two points in the 



path that form a segment colliding with an obstacle, as shown in figure 3.2 The second 



step in the function is a mutation operator that moves a point close to an obstacle to 
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a random point in the vicinity, as explained graphically in figure 3.3[ The mutation 



operator is inspired by the ones used in the Adaptive Evolutionary Planner/Navigator 
(EP/N) presented in |XMZT97] . while the arc operator is derived from the arc operator 
in the Evolutionary Algorithm presented in |AR05j . 

Algorithm 20 arc(path, firstCol) 
1: vicinity ^ some vicinity size 

2: randDev ^ random(— vicinity, vicinity) 

3: pointl ^ path[firstCol] 

4: point2 ^ path[firstCol + 1] 

5: if random()%2 then 

6: newPointl ^ (pointl[X] + randDev, point 

7: newPoint2 ^ (point2[X] + randDev, point2[y]) 

8: else 

9: newPointl ^ (point 1[X], point + randDev) 
10: newPoint2 ^ (point2[X], point2[F] + randDev) 

11: if path segments point l-newPointl-newPoint2-point2 are collision free then 
12: Add new points between pointl and point2 
13: else 

14: Drop new points 



Algorithm 21 mut (path, firstCol) 
1: vicinity ^ some vicinity size 

2: path[firstCol] [X] + = random(— vicinity, vicinity) 

3: path[firstCol] [Y] + = random(— vicinity, vicinity) 

4: if path segments before and after path[firstCol] are collision free then 
5: Accept new point 
6: else 

7: Reject new point 



The third and last stage is the greedy optimization heuristic, which can be seen as 
a post-processing for path shortening, that eliminates intermediate nodes if doing so 



does not create collisions, as is described in the algorithm 22 
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Algorithm 22 postProcess(pat/i) 



I: i^O 

2: while i < path. size() — 2 do 

3: if segment path[i] to path[i + 2] is collision free then 

4: Delete path[i+l] 

5: else 

6: i ^ i + 1 
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Figure 3.1: A Multi-stage Strategy for Dynamic Path Planning. This figure 
describes the life-cycle of the multi-stage algorithm presented here. The RRT, informed 
local search, and greedy heuristic are combined to produce a cheap solution to the 
dynamic path planning problem. 
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Figure 3.2: The arc operator. This operator draws an offset value A over a fixed 
interval called vicinity. Then, one of the two axes is selected to perform the arc and 
two new consecutive points are added to the path, rii is placed at a ±A of the point b 
and n2 at ±A of point c, both of them over the same selected axis. The axis, sign and 
value of A are chosen randomly from an uniform distribution. 
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Figure 3.3: The mutation operator. This operator draws two offset values and 
Ay over a vicinity region. Then the same point b is moved in both axes from b = [b^, by] 
to b' = [bx ± Aj., by ± Aj^], where the sign and offset values are chosen randomly from 
an uniform distribution. 
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Chapter 4 

Experimental Setup and Results 



4.1 Experimental Setup 

Although the algorithms developed in this thesis are aimed at dynamic environments, 
for the sake of completeness they will also be compared in partially known environments 
and in totally unknown environments, where some or all of the obstacles become visible 
to the planner as the robot approaches each one of them, simulating a robot with limited 
sensor range. 



4.1.1 Dynamic Environment 



The first environment for our experiments consists on two maps with 30 moving obsta- 
cles the same size of the robot, with a random speed between 10% and 55% the speed 
of the robot. Good performance in this environment is the main focus of this thesis. 



This dynamic environments are illustrated in figures 4.1 and 4.2 



4.1.2 Partially Known Environment 



The second environment uses the same maps, but with a few obstacles, three to four 
times the size of the robot, that become visible when the robot approaches each one of 
them. This is the kind of environment that most dynamic RRT variants were designed 



for. The partially known environments are illustrated in figure 4.3 and 4.4 
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Figure 4.1: The dynamic environment, map 1. The green square is our robot, currently 
at the start position. The blue squares are the moving obstacles. The blue cross is the 
goal. 



4.1.3 Unknown Environment 

For completeness sake, we will compare the different technique in a third environment, 
were we use one of the maps presented before, but all the obstacles will initially be 
unknown to the planners, and will become visible as the robot approaches them, forcing 
several re-plans. This unknown environment is illustrated in figure 4.5[ 



4.2 Implementation Details 

The algorithms where implemented in C++ using the MoPa framework^ partly devel- 
oped by the author. This framework features exact collision detection, three different 
map formats (including .pbm images from any graphic editor), dynamic, unknown and 
partially known environments and support for easily adding new planners. One of 
the biggest downsides is that it only supports rectangular objects, so several objects 

^MoPa homepage: https : //csrg. inf . utf sm. cl/twiki4/bin/view/CSRG/MoPa 
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Figure 4.2: The dynamic environment, map 2. The green square is our robot, currently 
at the start position. The blue squares are the moving obstacles. The blue cross is the 
goal. 



must be used to represent other geometrical shapes, as in figure AA. composed of 1588 
rectangular objects. 

There are several variations that can be found in the literature when implementing 
RRT. For all our RRT variants, the following are the details on where we departed 
from the basics: 

1. We always use two trees rooted at qinu and qgoai- 

2. Our EXTEND function, if the point cannot be added without collisions to a tree, 
adds the mid point between the nearest tree node and the nearest collision point 
to it. 

3. In each iteration, we try to add the new randomly generated point to both trees, 
and if successful in both, the trees are merged, as proposed in |KLOO] . 

4. We believe that there might be significant performance differences between al- 
lowing or not allowing the robot to advance towards the node nearest to the goal 
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Figure 4.3: The partially known environment, map 1. The green square is our robot, 
currently at the start position. The yellow squares are the suddenly appearing obsta- 
cles. The blue cross is the goal. 

when the trees are disconnected, as proposed in |ZKB07j . 

In point |4] above, the problem is that the robot would become stuck if it enters a small 
concave zone of the environment (like a room in a building) while there are moving 
obstacles inside that zone, but otherwise it can lead to better performance. Therefore 
we present results for both kinds of behavior: DRRT-adv and MP-RRT-adv move even 
when the trees are disconnected, while DRRT-noadv and MP-RRT-noadv only move 
when the trees are connected. 

In MP-RRT, the forest was handled by simply replacing the oldest tree in it if the 
forest had reached the maximum allowed size. 

Concerning the parameter selection, the probability for selecting a point in the vicin- 
ity of a point in the waypoint cache in DRRT was set to 0.4 as suggested in |FKS06j . 
The probability for trying to reuse a subtree in MP-RRT was set to 0.1 as suggested 
in |ZKB07j . Also, the forest size was set to 25 and the minimum size of a tree to be 
saved in the forest was set to 5 nodes. 
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Figure 4.4: The partially known environment, map 2. The green square is our robot, 
currently at the start position. The yellow squares are the suddenly appearing obsta- 
cles. The blue cross is the goal. 

For the combined RRT-EP/N, it was considered the planner was stuck after two sec- 
onds without a feasible solution in the population, at which point a new solution from 
a RRT variant is inserted into the population. For the simple multi-stage probabilis- 
tic algorithm, the restart is made after one second of encountering the same obstacle 
along the planned path. This second approach, which seems better, cannot be applied 
to the RRT-EP/N, because there is no single path to check for collisions, but instead 
a population of paths. The restart times where manually tuned. 

4.3 Results 



The three algorithms were run a hundred times in each environment and map com- 
bination. The cutoff time was five minutes for all tests, after which the robot was 
considered not to have reached the goal. Results are presented concerning: 



31 



Figure 4.5: The unknown environment. The green square is our robot, currently at 
the start position. The blue cross is the goaL None of the obstacles is visible initially 
to the planners 



• Success rate (S.R.): The percentage of times the robot arrived at the goal, before 
reaching the five minutes cutoff time. This does not account for collisions or time 
the robot was stopped waiting for a plan. 

• Number of nearest neighbor lookups performed by each algorithm (N.N.): One of 
the possible bottlenecks for tree-based algorithms 

• Number of collision checks performed ( C. C), which in our specific implementa- 
tion takes a significant percentage of the running time 

• Time it took the robot to reach the goal, ± the standard deviation. 



4.3.1 Dynamic Environment Results 



The results in tables 141] and show that the multi-stage algorithm takes considerably 
less time than the DRRT and MP-RRT to reach the goal, with far less collision checks. 
The combined RRT-EP/N is a close second. It was expected that nearest neighbor 
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lookups would be much lower in both combined algorithms than in the RRT variants, 
because they are only performed in the initial phase and restarts, not during navigation. 
The combined algorithms produce more consistent results within a map, as shown by 
their smaller standard deviations, but also across different maps. An interesting fact 
is that in map 1 DRRT is slightly faster than MP-RRT, and in map 2 MP-RRT is 
faster than DRRT. However the differences are too small to draw any conclusions. 



Figures 4^ and 4/7 show the times and success rates of the different algorithms, when 
changing the number of dynamic obstacles in map 1. The simple multi-stage algorithm 
and the mixed RRT-EP/N clearly show the best performance, while the DRRT-adv 
and MP-RRT-adv significantly reduce their success rate when confronted to more than 
30 moving obstacles. 



Table 4.1: Dynamic Environment Results, map 1. 



Algorithm 


S.R.[%] 


c.c. 


N.N. 


Time[s] 


Multi-stage 


99 


23502 


1122 


6.62 ± 0.7 


RRT-EP/N 


100 


58870 


1971 


10.34 ± 14.15 


DRRT-noadv 


100 


91644 


4609 


20.57 ± 20.91 


DRRT-adv 


98 


107225 


5961 


23.72 ± 34.33 


MP-RRT-noadv 


100 


97228 


4563 


22.18 ± 14.71 


MP-RRT-adv 


94 


118799 


6223 


26.86 ± 41.78 



Table 4.2: Dynamic Environment Results, map 2. 



Algorithm 


S.R.[%] 


C.C. 


N.N. 


Time[s] 


Multi-stage 


100 


10318 


563 


8.05 ± 1.47 


RRT-EP/N 


100 


21785 


1849 


12.69 ± 5.75 


DRRT-noadv 


99 


134091 


4134 


69.32 ± 49.47 


DRRT-adv 


100 


34051 


2090 


18.94 ± 17.64 


MP-RRT-noadv 


100 


122964 


4811 


67.26 ± 42.45 


MP-RRT-adv 


100 


25837 


2138 


16.34 ± 13.92 
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Figure 4.6: Times for different number of moving obstacles in map 1. 



4.3.2 Partially Known Environment Results 



Taking both maps into consideration, the results in tables 4^ and AA show that both 
combined algorithms are faster and more consistent than the RRT variants, with the 
simple multi-stage algorithm being faster in both. These results were unexpected, as 
the combined algorithms were designed for dynamic environments. It is worth to notice 
though, that in map 1 DRRT-adv is a close second, but in map 2 it is a close last, so its 
lack of reliability does not make it a good choice in this scenario. In this environment, 
as in the dynamic environment, in map 1 DRRT is faster than MP- RRT, while the 
opposite happens in map 2. 

4.3.3 Unknown Environment Results 



Results in table 4.5 present the combined RRT-EP/N clearly as the faster algorithm in 



unknown environments, with the multi-stage algorithm in second place. In contrast to 
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Figure 4.7: Success rate for different number of moving obstacles in map 1. 



Table 4.3: Partially Known Environment Results, map 1. 



Algorithm 


S.R.[%] 


C.C. 


N.N. 


Time[s] 


Multi-stage 


100 


12204 


1225 


7.96 ± 2.93 


RRT-EP/N 


99 


99076 


1425 


9.95 ± 2.03 


DRRT-noadv 


100 


37618 


1212 


11.66 ± 15.39 


DRRT-adv 


99 


12131 


967 


8.26 ± 2.5 


MP-RRT-noadv 


99 


49156 


1336 


13.82 ± 17.96 


MP-RRT-adv 


97 


26565 


1117 


11.12 ± 14.55 



dynamic and partially known environments in this same map, MP-RRT is faster than 
DRRT. 
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Table 4.4: Partially Known Environment Results, map 2. 



Algorithm 


S.R.[%] 


c.c. 


N.N. 


Time[s] 


Multi-stage 


100 


12388 


1613 


17.66 ± 4.91 


RRT-EP/N 


100 


42845 


1632 


22.01 ± 6.65 


DRRT-noadv 


99 


54159 


1281 


32.67 ± 15.25 


DRRT-adv 


100 


53180 


1612 


32.54 ± 19.81 


MP-RRT-noadv 


100 


48289 


1607 


30.64 ± 13.97 


MP-RRT-adv 


100 


38901 


1704 


25.71 ± 12.56 



Table 4.5: Unknown Environment Results 



Algorithm 


S.R.[%] 


C.C. 


N.N. 


Time[s] 


Multi-stage 


100 


114987 


2960 


13.97 ± 3.94 


RRT-EP/N 


100 


260688 


2213 


10.69 ± 2.08 


DRRT-noadv 


98 


89743 


1943 


18.38 ± 22.01 


DRRT-adv 


100 


104601 


2161 


19.64 ± 34.87 


MP-RRT-noadv 


99 


129785 


1906 


21.82 ± 27.23 


MP-RRT-adv 


100 


52426 


1760 


16.05 ± 10.87 
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Chapter 5 



Conclusions and Future Work 



The new multi-stage algorithm proposed here has good performance in very dynamic 
environments. It behaves particularly well when several small obstacles are moving 
around at random. This is explained by the fact that if the obstacles are constantly 
moving, they will sometimes move out of the way by themselves, which our algorithm 
takes advantage of, while RRT based ones do not, they just drop branches of the tree 
that could prove useful again just a few moments later. The combined RRT-EP/N, 
although having more operators, and automatic adjustment of the operator probabil- 
ities according to their effectiveness, is still better than the RRT variants, but about 
55% slower than the simple multi-stage algorithm. This is explained by the number of 
collision checks performed, more than twice than the multi-stage algorithm, because 
collision checks must be performed for the entire population, not just a single path. 

In the partially known environment, even though the difference in collision checks 
is even greater than in dynamic environments, the RRT-EP/N performance is about 
25% worse than the multi-stage algorithm. Overall, the RRT variants are closer to the 
performance of both combined algorithms. 

In the totally unknown environment, the combined RRT-EP/N is about 30% faster 
than the simple multi-stage algorithm, and both outperform the RRT variants, with 
much smaller times and standard deviations. 

All things considered, the simple multi-stage algorithm is the best choice in most 
situations, with faster and more predictable planning times, a higher success rate, fewer 
collision checks performed and, above all, a much simpler implementation than all the 
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other algorithms compared. 

This thesis shows that a muhi-stage approach, using different techniques for initial 
plannning and navigation, outperforms current probabilistic sampling techniques in 
dynamic, partially known and unknown environments. 

Part of the results presented in this thesis are published in |BALS09"] . 

5.1 Future Work 

We propose several areas of improvement for the work presented in this thesis. 

5.1.1 Algorithms 

The most promising area of improvement seems to be to experiment with different 
on-line planners such as a version of the EvP ( |AR05j and |AR08] ) modified to work 
in continuous configuration space or a potential field navigator. Also, the local search 
presented here could benefit from the use of more sophisticated operators and the 
parameters for the RRT variants (such as forest size for MP-RRT), and the EP/N (such 
as population size) could benefit from being tuned specifically for this implementation, 
and not simply reusing the parameters found in previous work. 

Another area of research that could be tackled is extending this algorithm to higher 
dimensional problems, as RRT variants are known to work well in higher dimensions. 

Finally, as RRT variants are suitable for kinodynamic planning, we only need to 
adapt the on-line stage of the algorithm to have a new multi-stage planner for problems 
with kinodynamic constraints. 

5.1.2 Framework 

The MoPa framework could benefit from the integration of a third party logic layer, 
with support for arbitrary geometrical shapes, a spatial scene graph and hierarchical 
maps. Some candidates would be OgreODE |Ugr| , Spring RTS |Spr| and ORTS |URTj . 

Other possible improvements are adding support for other map formats, including 
discrimination of static and moving obstacles, limited sensor range simulation and 
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integration with external hardware such as the Lego NXT Leg , to run experiments in 
a more reahstic scenario. 
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